Codecov Report

❌ Patch coverage is 95.83333% with 3 lines in your changes missing coverage. Please review.

... and 10 files with indirect coverage changes

@SteveMacenski sorry for late response. I made those changes you wanted.
In my local branch I attempted to add the current implementation to the controller server. I use cross product to decide left or right of the track. Added that function cross products to geomtery utils. If negative left positive right kind of distinction I decided to use float32 (cause publishing string is not a prefered practice ).
Added the node to bt_navigator by b. tree xml's with the help of an addition rate controller. Now it's making system recreate path but it doesn't resolve the distance with path trajectory. So I think adding progress checker and critics will complete this step.
If you think this is the right approach I would like to continue with this way pushing it.

I changed util's cmake according to @mini-1235 's warning.
@SteveMacenski as I said above I created a msg type that can provide things you wanted: current pose cross product and last segment index. Velocity can be added if you wish.
Cross product is to decide which side are we on of the track. A check for boundries of current_path_ could be added but added but I don't think it's needed cause they allready exist in util func.
About progress checkers: I couldn’t come up with a way to utilize the plugin style effectively, since we can publish data and track status from a BT node with more freedom. Please correct me if I’m wrong about the concepts.
In my plans checking the situation from a bt node and add that node to a tree like this way:

<RateController hz="3.0">
          <Fallback name="PathTrackingRecoveryPlanner">
            <IsWithinPathTrackingBounds max_tracking_error="2.0" />
            <Fallback name="TieredReplanning">
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
              <Sequence>
                <ClearEntireCostmap name="ClearGlobalCostmap-Tier2" service_name="global_costmap/clear_entirely_global_costmap"/>
                <RetryUntilSuccessful num_attempts="1">
                  <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
                </RetryUntilSuccessful>
              </Sequence>
            </Fallback>
          </Fallback>
        </RateController>

Adding clear costmap clearence is a future investment.
About critics I think going as proposed in the issue: averaging the deviation is the best way. would like to here your review.

Hello @SteveMacenski First of all I am not trying to rush the proccess but to inform you about my current local progress so when you have time for my committed codes it will allign with your vision and we can move faster.  

About DWB Critics: I added the critics and a unit test. I have been able to create a case where that critic can create a diff here are the proof vids and parameters I used


.

About Costmap Layer: I get results in here but incosistent. and to be honest I am not really sure about my expectations in here. First path is not getting a corridor but the second is answering. In addition sometimes I can not get response too. But when I do get response I think I get a decent corridor around the path ahead of robot.

BT node: This started to work better with others giving less frequent fails and now able to recover faster.

MPPI critic: This one is a bit problematic as mppi is using diff kind of trajectory units. So I either have to convert it (which kills entire philosphy) or create an overloaded function that uses it's input types. I would like to hear your guidance if there is a better and efficient approach.

I missed 1 of your comments ( the 1 about doxygen of the util func) but I don't like too many tiny commits I will make it better in next commit.

I commit as you give feedback so I can be in synch with you and we can progress step by step.

I tried to find places to utilize added functions. I think maybe distance_to_segment can be used in someplace but it can optimize some places if used in place euclidean distance or optimization in planner side of the repo . I am really not sure about it. Maybe create an rviz2 plugin

Thanks for your guidance and time from now.

@SteveMacenski Last local update:
I think I completed the code now. As you can see in the above, there are visual proofs, and I have added comments, logs, and unit test. In addition, I didn't make previous mistakes regarding lint, uncrust, and naming conventions. Of course, there can still be some reviews, but I believe this is working now.
Cheers.

My bad on the delayed reviews. I think this is still relevant though as a next step:

Great - so after these changes this should be usable to find the closest point on the path. Can you identify the places in the codebase today we check for such things (controller plugins, progress checkers, BT navigators, etc) and how we might use this utility? That seems like the next logical step to me.

Then, expose that feedback to the controller server, a new bt node to use this as well. Once all that is in place, the controller critics for MPPI/DWB would round it all off!

It sounds like you started on this, but I wanted to get a full list of possible uses before implementing to make sure we had the full list.

Rather than publishing as a topic, I think this would make sense to be a feedback element of the controller server that can be exposed to the behavior tree via the feedback for use. We can also have a BT node implemented that can compute it itself, I suppose. At that point, I think its worth going into the DWB/MPPI critics once we have something at the higher level -- though we could open the use of it as a separate PR so we can review that separately from the Controller Server / Behavior Tree integration points since those are nicely isolated from each other.

I think also publishing this as a topic isn't bad, so we can keep that too!

So, I think in summary, can you open a new PR called "Path Tracking Distance - Controller Plugins" with the DWB / MPPI work that we can review since you're far on that already?

Added the node to bt_navigator by b. tree xml's with the help of an addition rate controller. Now it's making system recreate path but it doesn't resolve the distance with path trajectory. So I think adding progress checker and critics will complete this step.

About progress checkers: I couldn’t come up with a way to utilize the plugin style effectively, since we can publish data and track status from a BT node with more freedom. Please correct me if I’m wrong about the concepts.

I'm not entirely sure what you mean by these. Can you expand?

About Costmap Layer: I get results in here but incosistent. and to be honest I am not really sure about my expectations in here. First path is not getting a corridor but the second is answering. In addition sometimes I can not get response too. But when I do get response I think I get a decent corridor around the path ahead of robot.

I think we should pause on this one at the moment and hone in on getting what you have started into the stack. The Controller critics for MPPI/DWB + the controller server / BT node is itself an accomplishment we can boast about 😉

But, if you wanted to look at it for a 3rd PR, basically: (a) create a new layer, (b) from a given path, compute the band of allowable area it can go from that path based on the max allowed deviation, (c) mark anything outside of this band as lethal space or very high space (parameterize the out-of-bounds cost).

This pull request is in conflict. Could you fix it @silanus23?

@silanus23, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@silanus23 please don't force push - it actually makes it much harder for me to review since I can't see diffs of what changed. :-)

Take a look at CI - a bunch of tests failed and might be caused by this https://app.circleci.com/pipelines/github/ros-navigation/navigation2/16107/workflows/df9141b5-4e21-482e-a3e7-aaba01c88569/jobs/47067/tests

Sorry about that. That's because I have gone through a Git accident. I will post the proper version soon.

@SteveMacenski I think I handled your requests. Added tracking error and publishing it there. I added velocity and distance to goal in tracking_error.msg too and deleted them from path follow to not duplicate. In advance the sign of the tracking_error is now deciding as you say.

I started to use pre_commit normally I use colcon test and it would show the wrongs in styling but this time didn't show. Can it be the problem?

Please fix the styling I pointed out. I'll look into it. It should have been automatically caught.

@silanus23 merged! I think next steps are the BT node (which should be easy thankfully) and perhaps an example XML for how to use this and we're off to the races to use it in the plugins/costmap!